/*
 * Author: 凌逆战 | Never
 * Data: 2023/7/5
 * Description:
 * https://github.com/muggot/openmcu/blob/97a9aa6130d8b4fe85483f5193e9b39abff1c84b/plugins/G722.2/AMR-WB/enc_util.c#L307
 */
#include "common.h"
#include "resampler.h"

// float up_buf_R[DOWN_BUFF_SIZE] = {0.0f};
// static float buf_R[DOWN_BUFF_SIZE] = {0.0f};
// static short demo_buffer[32];
// static short out_buffer[32];
//
// //static float mem_L[4 * ANKER_BIQUAD_ORDER];
// static float mem_R[4 * BIQUAD_ORDER];
//
// //static float mem_L_up[4 * ANKER_BIQUAD_ORDER];
// static float mem_R_up[4 * BIQUAD_ORDER];


/* 2-order IIR filter */
// 低通分子b是：1,2,1
// 高通分子b是：1,-2,1
void Biquad_filter(float* psignal, int lg, float* mem, float* sos) {
    int i;
    float x0, x1, x2, y0, y1, y2;
    float a1, a2, g, b;
    y1 = mem[0];
    y2 = mem[1];
    x0 = mem[2];
    x1 = mem[3];

    g = sos[0];
    a1 = sos[1];
    a2 = sos[2];
    for (i = 0; i < lg; i++) {
        x2 = x1;
        x1 = x0;
        x0 = psignal[i];
        b = x0 + 2 * x1 + x2;
        b *= g;

        y0 = b - a1 * y1 - a2 * y2;
        psignal[i] = y0;
        y2 = y1;
        y1 = y0;
    }

    mem[0] = ((y1 > (float)1e-10) | (y1 < (float)-1e-10)) ? y1 : 0;
    mem[1] = ((y2 > (float)1e-10) | (y2 < (float)-1e-10)) ? y2 : 0;
    mem[2] = ((x0 > (float)1e-10) | (x0 < (float)-1e-10)) ? x0 : 0;
    mem[3] = ((x1 > (float)1e-10) | (x1 < (float)-1e-10)) ? x1 : 0;
}


void Biquad_filter_ab(float* x, float* y, int lg, float* mem, float* a, float* b) {
    int i;
    float a1, a2, b0, b1, b2;
    float x1, x2, y1, y2;
    x1 = mem[0];
    x2 = mem[1];
    y1 = mem[2];
    y2 = mem[3];

    b0 = b[0];
    b1 = b[1];
    b2 = b[2];
    a1 = a[1];
    a2 = a[2];
    for (i = 0; i < lg; i++) {
        y[i] = (b0 * x[i] + b1 * x1 + b2 * x2
                - a1 * y1 - a2 * y2);
        x2 = x1;
        x1 = x[i];
        y2 = y1;
        y1 = y[i];
    }

    mem[0] = ((x1 > 1e-10) | (x1 < -1e-10)) ? x1 : 0;
    mem[1] = ((x2 > 1e-10) | (x2 < -1e-10)) ? x2 : 0;
    mem[2] = ((y1 > 1e-10) | (y1 < -1e-10)) ? y1 : 0;
    mem[3] = ((y2 > 1e-10) | (y2 < -1e-10)) ? y2 : 0;
}

/* 低通滤波器fs=96k, fc=7khz, order=2*4 */
int Lowpass7k_96k(float* pcmIn, int pcmLen, float* mem) {
    float sos[3];
    float* pmem = mem;

    /* low pass filter with cutoff frequency of 2000Hz */
    sos[0] = 0.047467810225879183f;    //g
    sos[1] = -1.6512641070675567f;    //a1
    sos[2] = 0.84113534797107326f;    //a2
    Biquad_filter(pcmIn, pcmLen, pmem, sos);

    pmem += 4;
    sos[0] = 0.041392550991654772f;    //g
    sos[1] = -1.4399238858340904f;    //a1
    sos[2] = 0.6054940898007094f;    //a2
    Biquad_filter(pcmIn, pcmLen, pmem, sos);

    pmem += 4;
    sos[0] = 0.037699611840990042f;    //g
    sos[1] = -1.3114575032464098f;   //a1
    sos[2] = 0.46225595061036978f;    //a2
    Biquad_filter(pcmIn, pcmLen, pmem, sos);

    pmem += 4;
    sos[0] = 0.035963161068487279f;    //g
    sos[1] = -1.251051539274634f;   //a1
    sos[2] = 0.39490418354858314f;    //a2
    Biquad_filter(pcmIn, pcmLen, pmem, sos);

    return 0;
}

/* HighPass biquad filter; 16kHz,fc=70Hz,order=2 */
void HighPass_fc70kHz(float* pcmIn, int pcmLen, float* mem) {
    float sos[3] = {0};
    float* pmem = mem;

    sos[0] = 0.9862119246271f;    //g
    sos[1] = -1.9722337291953f;    //a1
    sos[2] = 0.9726139693131f;    //a2
    Biquad_filter(pcmIn, pcmLen, pmem, sos);
}


void Inter16kto96k(float* buf, int len_x, float* old_buf_end, float* inter_buf) {
    int i, r;
    float tmp;
    for (i = 0, r = 0; i < len_x; i++, r += 6) {
        if (i == 0) {
            tmp = (buf[0] - *old_buf_end) / 6;
            inter_buf[r] = *old_buf_end;
            inter_buf[r + 1] = *old_buf_end + tmp;
            inter_buf[r + 2] = *old_buf_end + 2 * tmp;
            inter_buf[r + 3] = *old_buf_end + 3 * tmp;
            inter_buf[r + 4] = *old_buf_end + 4 * tmp;
            inter_buf[r + 5] = *old_buf_end + 5 * tmp;
        } else {
            tmp = (buf[i] - buf[i - 1]) / 6;
            inter_buf[r] = buf[i - 1];
            inter_buf[r + 1] = buf[i - 1] + tmp;
            inter_buf[r + 2] = buf[i - 1] + 2 * tmp;
            inter_buf[r + 3] = buf[i - 1] + 3 * tmp;
            inter_buf[r + 4] = buf[i - 1] + 4 * tmp;
            inter_buf[r + 5] = buf[i - 1] + 5 * tmp;
        }
    }
    *old_buf_end = buf[31];
}